/*******************************************************************************
* Copyright 2017 CLNST CO., LTD.
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
*     http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.

*******************************************************************************/
#include <rocr6_driver/rocr6-ctrl.h>

#define TIMEOUT_SEC(buflen,baud) (buflen*20/baud+2)             //接收超时
#define TIMEOUT_USEC 0

#define BUFFER_SIZE     18
#define DATA_GAIN           10

serial::Serial ser;

static unsigned char sendbuf[BUFFER_SIZE];
static unsigned char recvbuf[BUFFER_SIZE];

/*++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
                                                                    机械臂扩展接口
++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++*/
static double msg[6] = {1, 0, 0, 0, 0, 0}; 

/**
  * @brief  数据帧校验
  * @param  消息数据
  * @retval 校验结果
  */
static int uart_cksm(const unsigned char buf[18])
{
	int tmp = 0;
	int cs = 0;
	int i;
	for (i = 0; i < 16; i += 2)
	{
		tmp = buf[i + 1];
		tmp <<= 8;
		tmp += buf[i];
		cs += tmp;
	}
	tmp = buf[17];
	tmp <<= 8;
	tmp += buf[16];
	if ((unsigned short)cs == tmp)
	{
		return 1;
	}
	return 0;
}

/**
  * @brief  机械臂发送数据转换
  * @param  输入弧度
  * @retval 变换后的角度
  */
double rocr6_tf_tdata(double dt)
{
  return dt*DATA_GAIN*180/PI;
}

/**
  * @brief  机械臂接收数据转换
  * @param  输入角度
  * @retval 变换后的弧度
  */
 double rocr6_tf_rdata(double dt)
 {
   return dt/DATA_GAIN*PI/180;
 }

/**
  * @brief  机械臂发送角度数据
  * @param  命令，参数，角度值
  * @retval 实际发送的数据
  */
int rocr6_txSig(unsigned short cmd, unsigned short index, const double dt[6])
{
  short t_data;
	int i, j=0;
	unsigned int cs;
	sendbuf[0] = (unsigned char)cmd;
	sendbuf[1] = (unsigned char)(cmd >> 8u);
	cs = cmd;
	sendbuf[2] = (unsigned char)index;
	sendbuf[3] = (unsigned char)(index >> 8u);
	cs += index;

	for (i = 0; i < 12; i += 2)
	{
		t_data = (short)dt[j];
		cs += (unsigned short)t_data;
		sendbuf[i+4] = (unsigned char)t_data;
		sendbuf[i+5] = (unsigned char)(t_data >> 8u);
		j++;
	}

    sendbuf[16] = (unsigned char)cs;
    sendbuf[17] = (unsigned char)(cs >> 8u);

/*
    printf("txd:[ ");
    for(int k=0; k<18;k++)
    {
        printf("0x%02x ",sendbuf[k]);
    }
    printf("\r\n");
*/

    return ser.write(sendbuf, BUFFER_SIZE);
    return 0;
}

/**
  * @brief  机械臂接收角度数据
  * @param  接收到的角度
  * @retval 机械臂运行状态
  */
int rocr6_rxSig(double dt[6])
{
    short t_data;
    short cmd = 0;
    short flag = 0;
    int i, j = 0;

    if(ser.read(recvbuf, BUFFER_SIZE))
    {
        if(uart_cksm(recvbuf))
        {
            t_data = recvbuf[1];
		    t_data <<= 8;
	        t_data += recvbuf[0];
	        cmd = t_data;
            // printf("rxd:[ ");
            // for(int k=0; k<18;k++)
            // {
            //     printf("0x%02x ",recvbuf[k]);
            // }
            // printf("\r\n");
            if(cmd==0x5a)
            {
                t_data = recvbuf[3];
		        t_data <<= 8;
	            t_data += recvbuf[2];
	            flag = t_data;
	            for (i = 0; i < 12; i += 2)
		        {
		            t_data = recvbuf[i + 5];
		            t_data <<= 8;;
			        t_data += recvbuf[i + 4];
			        dt[j] = t_data;
			        j++;
		        }
            }
        }
    }
    return flag;
}

/**
  * @brief  机械臂抱闸设置
  * @param  无
  * @retval 无
  */
void rocr6_brake(unsigned char flag)
{
  msg[1] = flag%2;
  rocr6_txSig(0x80,0,msg); 
  usleep(10);
}

/**
  * @brief  机械臂广播设置
  * @param  无
  * @retval 无
  */
void rocr6_broadcast(unsigned char flag)
{
  msg[1] = flag%2;
  rocr6_txSig(0xff,0,msg); 
  usleep(10);
}

/**
  * @brief  机械臂回HOME位置
  * @param  无
  * @retval 无
  */
void rocr6_home_config(void)
{
  rocr6_txSig(1,0,msg);
  printf("goto home config.\r\n");
  usleep(10);
}

/**
  * @brief  启动机械臂连接
  * @param  无
  * @retval 初始化结果
  */
int rocr6_init(const char * dev)
{
    try 
    { 
    //设置串口属性，并打开串口 
        ser.setPort(dev); 
        ser.setBaudrate(115200); 
        serial::Timeout to = serial::Timeout::simpleTimeout(50); 
        ser.setTimeout(to); 
        ser.open();
        printf("[ROCR6]: link[%s] ---> ok\r\n",dev); 
    } 
    catch (serial::IOException& e) 
    { 
        printf("Unable to open port\r\n"); 
        return 0; 
    } 
  return 1;
}

/**
  * @brief 断开机械臂连接
  * @param  无
  * @retval 无
  */
void rocr6_deinit(void)
{
  printf("[ROCR6]: close.\r\n");
  if(ser.isOpen())
  {
    ser.close();
  }
}

/************************ (C) COPYRIGHT HopeMotion *****END OF FILE****/

